#include <string.h>
#include "ScopedLock.h"
#include "Mutex.h"
#include "Semaphore.h"
#include "Queue.h"

Queue::Queue(uint32_t maxSize, uint32_t messageSize) 
	: maxSize_(maxSize)
	, messageSize_(messageSize) 
	, head_(0)
	, tail_(0)
	, buf_(new uint8_t[messageSize * maxSize])
	, mutex_(new Mutex())
	, getSemaphore_(new Semaphore())
	, putSemaphore_(new Semaphore(maxSize - 1))
{}

Queue::~Queue() {
	delete putSemaphore_;
	delete getSemaphore_;
	delete mutex_;
	delete [] buf_;
}

bool Queue::get(void *message, uint32_t timeout) {
	if (!getSemaphore_->take(timeout))
		return false;
	ScopedLock<Mutex> l(mutex_);
	if (message != NULL)
		memcpy(message, buf_ + head_ * messageSize_, messageSize_);
	if (++head_ == maxSize_)
		head_ = 0;
	putSemaphore_->give();
	return true;
}

bool Queue::put(const void *message, uint32_t timeout) {
	if (!putSemaphore_->take(timeout))
		return false;
	ScopedLock<Mutex> l(mutex_);
	memcpy(buf_ + tail_ * messageSize_, message, messageSize_);
	if (++tail_ == maxSize_)
		tail_ = 0;
	getSemaphore_->give();
	return true;
}

void Queue::flush() {
	while(get(NULL, 0)) {}
}

